package com.pudu.json;

import android.util.Log;

import com.DataProcess.Robot;
import com.pudu.pdrobot.App;

import org.json.JSONException;
import org.json.JSONObject;

/**
 *
 * Created by Cyc on 2016/9/14.
 */
public class Json2PC {

    static String thisLogTag = "Json2PC";

    public  static void sendUWBS(double[] uwbs) {
        JSONObject jsonObject = new JSONObject();
        JSONObject jsonObject1 = new JSONObject();
        try {
            for(int i=0; i<uwbs.length; i++)
                jsonObject1.put(String.format("%d",i),uwbs[i]);
            jsonObject.put("type","uwb");
            jsonObject.put("distance",jsonObject1);
            Log.i(thisLogTag,"send json,uwb:"+jsonObject.toString());
        } catch (JSONException e) {
            e.printStackTrace();
        }
        App.instance.mMyLtsServer.Send(jsonObject.toString());
    }

    static long timeLast = System.currentTimeMillis();
    public static void sendPose() {
        long time = System.currentTimeMillis();

        JSONObject jsonObject = new JSONObject();
        PuduJson puduJson = new PuduJson();
        try {
            jsonObject.put("type","pose");
            jsonObject.put("pos",puduJson.buildCoordinate(Robot.position));
            jsonObject.put("dir",puduJson.buildCoordinate(Robot.dir));
            jsonObject.put("aim",puduJson.buildCoordinate(Robot.aimPoint));
            jsonObject.put("step_time",new JSONObject().put("ms",time - timeLast));
            Log.i(thisLogTag,"send json,pose:"+jsonObject.toString());
        } catch (JSONException e) {
            e.printStackTrace();
        }
        App.instance.mMyLtsServer.Send(jsonObject.toString());

        timeLast = time;
    }

    public static void sendPID(double speedL, double speedR, double targetL, double targetR) {
        String string = String.format("{\"type\":\"PID\", \"SPD\":{\"L\":%.3f,\"R\":%.3f}, \"TAR\":{\"L\":%.3f,\"R\":%.3f}}",
                speedL, speedR, targetL, targetR);
        App.instance.mMyLtsServer.Send(string);
        Log.i(thisLogTag,"send json,PID:"+string);
    }

    public static void sendYaw() {
        JSONObject jsonObject = new JSONObject();

        try {
            jsonObject.put("type","omega");
            jsonObject.put("imu",Robot.angleVel[2]);
            jsonObject.put("odom",Robot.axialVel);
            jsonObject.put("gyro",Robot.gyroAngleVel[2]);
        } catch (JSONException e) {
            e.printStackTrace();
        }

        App.instance.mMyLtsServer.Send(jsonObject.toString());
        Log.i(thisLogTag,"send json,yawSpeed:"+jsonObject.toString());
    }

    public static void sendTag() {
        JSONObject jsonObject = new JSONObject();
        PuduJson puduJson = new PuduJson();
        try {
            jsonObject.put("type","tag");
            jsonObject.put("pos",puduJson.buildCoordinate(Robot.tagPosition));
            Log.i(thisLogTag,"send json,tag:"+jsonObject.toString());
        } catch (JSONException e) {
            e.printStackTrace();
        }
        App.instance.mMyLtsServer.Send(jsonObject.toString());
    }

}
